from maix import camera, display, image, nn, app, pinmap, time, uart, touchscreen, thread
from maix._maix.image import Line

'''
协议
检测到最开始的目标药房号：0x0a 0x0b 0x01 target 0x0c
在路上检测到的目标药房号：0x0a 0x0b 0x02 (0x00:L、0x01:R) 0x0c
识别到停车点：           0x0a 0x0b 0x03 0x00 0x0c
'''
# 初始化串口
device = "/dev/ttyS0"
serial0 = uart.UART(device, 115200)
# 加载模型
detector = nn.YOLOv5(model="/root/models/model-120311(number)/model_120311.mud", dual_buff=True)
# 打开摄像头并显示
cam = camera.Camera(680, 480, detector.input_format())
dis = display.Display()
# 初始化按键扫描对象
ts = touchscreen.TouchScreen()
X_btn_pos = [620, 0, 60, 60]
X_btn_disp_pos = image.resize_map_pos(cam.width(), cam.height(), dis.width(), dis.height(), image.Fit.FIT_CONTAIN, X_btn_pos[0], X_btn_pos[1], X_btn_pos[2], X_btn_pos[3])

Switch_btn_pos = [620, 100, 60, 60]
Switch_btn_disp_pos = image.resize_map_pos(cam.width(), cam.height(), dis.width(), dis.height(), image.Fit.FIT_CONTAIN, Switch_btn_pos[0], Switch_btn_pos[1], Switch_btn_pos[2], Switch_btn_pos[3])

JIAN_btn_pos = [620, 420, 60, 60]
JIAN_btn_disp_pos = image.resize_map_pos(cam.width(), cam.height(), dis.width(), dis.height(), image.Fit.FIT_CONTAIN, JIAN_btn_pos[0], JIAN_btn_pos[1], JIAN_btn_pos[2], JIAN_btn_pos[3])
JIA_btn_pos = [560, 420, 60, 60]
JIA_btn_disp_pos = image.resize_map_pos(cam.width(), cam.height(), dis.width(), dis.height(), image.Fit.FIT_CONTAIN, JIA_btn_pos[0], JIA_btn_pos[1], JIA_btn_pos[2], JIA_btn_pos[3])

FilterJia_btn_pos = [0,420,60,60]
FilterJia_btn_disp_pos = image.resize_map_pos(cam.width(), cam.height(), dis.width(), dis.height(), image.Fit.FIT_CONTAIN, FilterJia_btn_pos[0], FilterJia_btn_pos[1], FilterJia_btn_pos[2], FilterJia_btn_pos[3])
FilterJian_btn_pos = [60,420,60,60]
FilterJian_btn_disp_pos = image.resize_map_pos(cam.width(), cam.height(), dis.width(), dis.height(), image.Fit.FIT_CONTAIN, FilterJian_btn_pos[0], FilterJian_btn_pos[1], FilterJian_btn_pos[2], FilterJian_btn_pos[3])
#************************************************************
Start_find_blob = 0     # 开始识别色块标志位
delay_flag = 0
t = 0
s = 0
target = 0      # 目标药房
L = 0           # 左转向
R = 1           # 右转向
direction = 2   # 转向标志
MAXcount = 10               # 滤波宏
MINcount = 5            
Filter_flag = 0
MAX_road_count = 4               # 滤波宏
MIN_road_count = 2            
max_count = MAXcount        # 滤波系数
min_count = MINcount
count = 0       # 统计计数
stop_flag = 0   # 停车标志
blobs_quantity = 0  # 记录块的数量
blobs_max = 5       # 最多出现色块数量
number_list = list(range(max_count))                # 保存每次识别的结果
tinydict = {1:0, 2:0, 3:0, 4:0, 5:0, 6:0, 7:0, 8:0} # 保存每个数字对应的出现次数
number_name = [' one',' two',' three',' four',' five','six',' seven',' eight']
#************************************************************

# 显示屏显示数据
def display_data(img):
    global target,direction,L,R,blobs_quantity,stop_flag,Filter_flag
    # 显示参数
    img.draw_string(0,8,"TAR:{}".format(target),color = image.COLOR_RED,scale=4,thickness=3)
    if direction == L:
        img.draw_string(0,58*1,"DIR:L",color = image.COLOR_BLUE,scale=4,thickness=3)
    elif direction == R:
        img.draw_string(0,58*1,"DIR:R",color = image.COLOR_BLUE,scale=4,thickness=3)
    else:
        img.draw_string(0,58*1,"DIR:",color = image.COLOR_BLUE,scale=4,thickness=3)
    img.draw_string(0,48*2+10,"blobs:{}".format(blobs_quantity),color = image.COLOR_GREEN,scale=2)
    img.draw_string(0,48*3,"blobs_M:{}".format(blobs_max),color = image.COLOR_RED,scale=2)
    img.draw_string(0,48*4,"Max_c:{}".format(max_count),color = image.COLOR_GREEN,scale=2)
    img.draw_string(0,48*5,"Min_c:{}".format(min_count),color = image.COLOR_GREEN,scale=2)
    img.draw_string(0,48*6,"stop:{}".format(stop_flag),color = image.COLOR_RED,scale=2)
    if Filter_flag == 0:
        img.draw_string(0,48*7,"road_Max:{}".format(MAX_road_count),color = image.COLOR_RED,scale=2)
        img.draw_string(0,48*8,"road_Min:{}".format(MIN_road_count),color = image.COLOR_GREEN,scale=2)
    else:
        img.draw_string(0,48*7,"road_Max:{}".format(MAX_road_count),color = image.COLOR_GREEN,scale=2)
        img.draw_string(0,48*8,"road_Min:{}".format(MIN_road_count),color = image.COLOR_RED,scale=2)
    # 画按键
    img.draw_string(X_btn_pos[0], X_btn_pos[1]+10, "X", image.COLOR_WHITE,scale=4)
    img.draw_rect(X_btn_pos[0], X_btn_pos[1], X_btn_pos[2], X_btn_pos[3],  image.COLOR_WHITE, 2)
    img.draw_string(Switch_btn_pos[0], Switch_btn_pos[1]+10, "Q", image.COLOR_RED,scale=4)
    img.draw_rect(Switch_btn_pos[0], Switch_btn_pos[1], Switch_btn_pos[2], Switch_btn_pos[3],  image.COLOR_RED, 2)
    img.draw_string(JIA_btn_pos[0], JIA_btn_pos[1], "+", image.COLOR_WHITE,scale=4)
    img.draw_rect(JIA_btn_pos[0], JIA_btn_pos[1], JIA_btn_pos[2], JIA_btn_pos[3],  image.COLOR_WHITE, 2)
    img.draw_string(JIAN_btn_pos[0], JIAN_btn_pos[1], "-", image.COLOR_WHITE,scale=4)
    img.draw_rect(JIAN_btn_pos[0], JIAN_btn_pos[1], JIAN_btn_pos[2], JIAN_btn_pos[3],  image.COLOR_WHITE, 2)
    img.draw_string(FilterJia_btn_pos[0], FilterJia_btn_pos[1], "+", image.COLOR_WHITE,scale=4)
    img.draw_rect(FilterJia_btn_pos[0], FilterJia_btn_pos[1], FilterJia_btn_pos[2], FilterJia_btn_pos[3],  image.COLOR_RED, 2)
    img.draw_string(FilterJian_btn_pos[0], FilterJian_btn_pos[1], "-", image.COLOR_WHITE,scale=4)
    img.draw_rect(FilterJian_btn_pos[0], FilterJian_btn_pos[1], FilterJian_btn_pos[2], FilterJian_btn_pos[3],  image.COLOR_RED, 2)

# 更新参数
def Update_parament():
    global target,max_count,min_count,count,number_list,direction,stop_flag
    target = 0      
    direction = 2   
    stop_flag = 0
    count = 0
    max_count = MAXcount
    min_count = MINcount
    number_list = list(range(max_count)) 

# 按键检测
def is_in_button(x, y, btn_pos):
    return x > btn_pos[0] and x < btn_pos[0] + btn_pos[2] and y > btn_pos[1] and y < btn_pos[1] + btn_pos[3]
def Key_scan():
    global blobs_max,Start_find_blob,Filter_flag,MAX_road_count,MIN_road_count
    x, y, pressed = ts.read()
    if is_in_button(x, y, X_btn_disp_pos) and pressed == 1:
        # 系数重置
        Update_parament()
        Start_find_blob = 0
    elif is_in_button(x, y, JIA_btn_disp_pos) and pressed == 1:
        blobs_max += 1
        time.sleep_ms(100)
    elif is_in_button(x, y, JIAN_btn_disp_pos) and pressed == 1:
        blobs_max -= 1
        time.sleep_ms(100)
    elif is_in_button(x, y, FilterJia_btn_disp_pos) and pressed == 1:
        if Filter_flag == 1:
            MAX_road_count += 1
        else:
            if MAX_road_count > MIN_road_count:
                MIN_road_count += 1
        time.sleep_ms(100)
    elif is_in_button(x, y, FilterJian_btn_disp_pos) and pressed == 1:
        if Filter_flag == 1:
            if MAX_road_count > MIN_road_count:
                MAX_road_count -= 1
        else:
            MIN_road_count -= 1
        time.sleep_ms(100)
    elif is_in_button(x, y, Switch_btn_disp_pos) and pressed == 1:
        if Filter_flag == 1:
            Filter_flag = 0
        else:
            Filter_flag = 1
        time.sleep_ms(100)

# 获取中心点坐标
def Get_center_point(x,y,w,h):
    return [x+w//2,y+h//2]

# 串口发送
def Usart_send(command):
    data = [0x0a,0x0b]
    for c in command:
        data.append(c)
    data.append(0x0c)
    serial0.write(bytes(data))
    #print(data)

# 查看停车点
stop_site = 0   # 统计识别到的停车点次数，识别到两次清除target
def detect_stop_site(img):
    global blobs_quantity,stop_flag,stop_site,Start_find_blob
    thresholds = [[0, 20, -10, 10, -15, 15]]
    if Start_find_blob != 0:
        ROI = [cam.width()//8, cam.height()//3, cam.width()//8*6, cam.height()-cam.height()//3]
        img.draw_rect(ROI[0],ROI[1],ROI[2],ROI[3], color = image.COLOR_RED)
        blobs = img.find_blobs(thresholds, pixels_threshold=1500,roi=ROI)
        jian = 0
        for blob in blobs:
            # 根据面积和长宽比例判断停车点的黑色色块
            if blob.pixels() > 4000:
                jian += 1
                img.draw_rect(blob[0], blob[1], blob[2], blob[3], image.COLOR_RED)
                continue
            else:
                proporation = blob[2]/blob[3] if blob[2] > blob[3] else blob[3]/blob[2]
                if proporation > 1.7:
                    jian += 1
                    img.draw_rect(blob[0], blob[1], blob[2], blob[3], image.COLOR_RED)
                    continue
            img.draw_rect(blob[0], blob[1], blob[2], blob[3], image.COLOR_GREEN)
        blobs_quantity = len(blobs) - jian
        if blobs_quantity >= blobs_max:  # 发送停车指令
            stop_site += 1
            time.sleep_ms(600)
            stop_flag = 1
            for i in range(0,5):
                Usart_send([0x03,0x00])
                time.sleep_ms(10)
            # 这里防止多次识别停车点
            delay_s(3)
            if stop_site >= 2:
                stop_site = 0
                Update_parament()
        else :
            stop_flag = 0
    else :
        blobs_quantity = 0
        
# 识别数字
def detector_number(img):
    global target,cam,direction,L,R,count,number_list,max_count,min_count,number_name,Start_find_blob
    # if target == 0:
    #     img.draw_rect(targer_ROI[0], targer_ROI[1], targer_ROI[2], targer_ROI[3],  image.COLOR_WHITE, 2)
    #     target_img = img.copy().crop(targer_ROI[0],targer_ROI[1],targer_ROI[2],targer_ROI[3])
    #     objs = detector.detect(target_img, conf_th = 0.7, iou_th = 0.45) # conf_th:置信度
    # else:
    objs = detector.detect(img, conf_th = 0.6, iou_th = 0.45) # conf_th:置信度
    number = 0
    number_lis = []
    for obj in objs:
        img.draw_rect(obj.x, obj.y, obj.w, obj.h, color = image.COLOR_RED)
        msg = f'{detector.labels[obj.class_id]}: {obj.score:.2f}'
        img.draw_string(obj.x, obj.y, msg, color = image.COLOR_RED)
        msg = f'area:{obj.w*obj.h}'
        img.draw_string(obj.x, obj.y+20, msg, color = image.COLOR_RED)
        number_lis.append(obj)
    # 统计判决
    number_list[count] = number_lis
    count+=1
    if count >= max_count:
        count = 0
        # 字典计算数字出现次数
        tinydict = {1:0, 2:0, 3:0, 4:0, 5:0, 6:0, 7:0, 8:0}
        tinydict_coor = {1:0, 2:0, 3:0, 4:0, 5:0, 6:0, 7:0, 8:0}
        for number_l in number_list:
            for n in number_l:
                number = number_name.index(detector.labels[n.class_id]) + 1
                tinydict[number] += 1
                tinydict_coor[number] = n   # 记录坐标
        print(tinydict)                     # 显示识别结果
        for key in tinydict.keys():
            if tinydict[key] >= min_count:
                if target == 0:
                    target = key
                    Start_find_blob = 1     # 开始识别停车点色块
                    max_count = MAX_road_count           # 降低统计系数
                    min_count = MIN_road_count
                    for i in range(0,5):
                        Usart_send([0x01,target])
                        time.sleep_ms(10)
                    delay_s(3)
                else :
                    if key == target:
                        center = Get_center_point(tinydict_coor[key].x,tinydict_coor[key].y,tinydict_coor[key].w,tinydict_coor[key].h)
                        if center[0] < cam.width()//2:  # L
                            direction = L
                        else:                           # R
                            direction = R
                        Usart_send([0x02,direction])
        number_list = list(range(max_count))            # 初始化列表

def delay_s(delay_t):
    global delay_flag,t,s
    delay_flag = 1
    s = delay_t
    t = 0  

while not app.need_exit():
    img = cam.read()
    if delay_flag == 0:
        detector_number(img)        # 识别数字
        detect_stop_site(img)       # 查找停车点
        Key_scan()                  # 按键检测
    else:
        time.sleep_ms(10)
        t += 1
        if t > 40*s:
            t = 0
            s = 0
            delay_flag = 0
        print(t)
    display_data(img)           # 显示数据
    dis.show(img)
